//vel2pwm_odom节点，订阅/cmd_vel和/odom_ekf，定时发布/pwm。
//有用的数据仅为x轴速度和z轴角速度
// 打角115为正，160左40度，70右40度
// 速度1520不动，1525为0.2但有响声，1530为0.6，1540为0.7，1550为1.0，1560可到1.8左右不过加速还要一段距离，1565为2左右

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64.h>

double speed;
double angle;
short odomFlag; //0第一圈普通，1第二圈普通，2直角弯，3第二圈终点
bool maplive=1;

void TwistCallback(const geometry_msgs::Twist& twist)
{
    angle=twist.angular.z;
    speed=twist.linear.x;
}

void DistanceCallback(const std_msgs::Float64& distance)
{
    double d=distance.data;
    if(d> 39.5) //第二圈
    {
        if(maplive)
        {
            // system("rosrun map_server map_saver");
            maplive = 0;
            ROS_INFO("Round 2 begins!");
            // system("rosnode kill map_node");
            // system("rosnode kill map_node");
        }

        if(d> 40 && d< 40.5) //第二圈直角弯
        {
            odomFlag = 2;
        }
        else if(d> 80) //第二圈已跑完
        {
            odomFlag = 3;
        }
        else
        {
            odomFlag = 1;
        }
    }
    else //第一圈
    {
        if(d > 2.6 && d< 3.1) //第一圈直角弯
        {
            odomFlag = 2;
        }
        else
        {
            odomFlag = 0;
        }
    }
}

double speedcontrol(bool round2)
{
    if(angle >= 0.7 || angle <= -0.7) //打角超限
    {
        if(round2)
            return 1555;
        else
            return 1555;
    }
    else if(angle >= 0.35 || angle <= -0.35) //超20度
    {
        if(round2)
            return 1565;
        else
            return 1560;
    }
    else 
    {
        if(round2)
            return 1570;
        else
            return 1565;
    }
}

double anglecontrol()
{
    if(angle >= 0.7) return(160); //限幅左右40度
    else if(angle <= -0.7) return(70);
    else return(angle*64.46+115); //*180/pi*45/40，即弧度制转角度制再转pwm
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "vel2pwm_odom");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/cmd_vel",1,TwistCallback);
    ros::Subscriber sub2 = nh.subscribe("/distance",1,DistanceCallback);
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>( "/pwm", 1 );
    geometry_msgs::Twist pwm;
    ros::Rate r(16);
    ROS_INFO("vel2pwm_odom start successfully.");
    while(ros::ok())
    {
        ros::spinOnce();
        if(odomFlag == 0) //0第一圈普通，1第二圈普通，2直角弯，3第二圈终点
        {
            pwm.linear.x=speedcontrol(0);
            pwm.angular.z=anglecontrol();
        }
        else if(odomFlag == 1)
        {
            pwm.linear.x=speedcontrol(1);
            pwm.angular.z=anglecontrol();
        }
        else if(odomFlag == 2)
        {
            pwm.linear.x=speedcontrol(0);
            pwm.angular.z = 150; //向左打
            ROS_INFO("Right angle!");
        }
        else if(odomFlag == 3)
        {
            pwm.linear.x=1520;
            pwm.angular.z=anglecontrol();
            ROS_INFO("Finished!");
        }
        
        pub.publish(pwm);
        r.sleep();
    }
    return 0;
}